#! /usr/bin/env python
# -*- coding: UTF-8 -*- 

import rospy 
import tf2_ros
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped

"""
    发布方：发布两个坐标系的相对关系 （车辆底盘-----base_link 和雷达 ---laser ）
    流程：
        1. 导包
        2.初始化节点
        3.创建订阅对象
        4.组织被软件的坐标数据 
        5.转换逻辑实现，调用 tf 封装的算法
        6.spin()
"""


if __name__ == "__main__":

    # 2.初始化节点
    rospy.init_node("static_sub_p")

    # 3.创建订阅对象
    # 3-1 创建缓存对象
    buffer =  tf2_ros.Buffer()
    # 3-2  创建订阅对象（将缓存传入）
    sub = tf2_ros.TransformListener(buffer)

    # 4.组织被软件的坐标数据 
    ps = PointStamped()
    ps.header.stamp = rospy.Time.now()
    ps.header.frame_id = "laser"
    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0

    # 5.转换逻辑实现，调用 tf 封装的算法
    rate = rospy.Rate(2)
    while not rospy.is_shutdown():
        try:
            # 转换实现
            point_target = buffer.transform(ps, "base_link")
            rospy.loginfo("转换结果：x = %.2f, y = %.2f, z = %.2f, 参考的坐标系：%s", 
                        point_target.point.x,
                        point_target.point.y,
                        point_target.point.z, 
                        point_target.header.frame_id)
            
            rate.sleep()
        except Exception as e:
            rospy.logwarn("错误提示：%s", e)
    # 6.spin()